#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <iostream>

int main(int argc, char *argv[])
{
  ros::init(argc, argv, "fake_vicon");
  ros::NodeHandle nh;
  ros::Publisher pos_pub = nh.advertise<geometry_msgs::PoseStamped>("/pose", 1);
  ros::Publisher vel_pub = nh.advertise<geometry_msgs::TwistStamped>("/vel", 1);
  ros::Rate loop_rate(50);

  geometry_msgs::PoseStamped pos;
  geometry_msgs::TwistStamped vel;

  pos.pose.position.x = 0.0;
  pos.pose.position.y = 0.0;
  pos.pose.position.z = 1.0;
  pos.pose.orientation.x = 0.0;
  pos.pose.orientation.y = 0.0;
  pos.pose.orientation.z = 0.0;
  pos.pose.orientation.w = 1.0;

  vel.twist.linear.x = 0.0;
  vel.twist.linear.y = 0.0;
  vel.twist.linear.z = 0.0;
  vel.twist.angular.x = 0.0;
  vel.twist.angular.y = 0.0;
  vel.twist.angular.z = 0.0;

  while (ros::ok())
  {
    pos.header.stamp = vel.header.stamp = ros::Time::now();

    pos_pub.publish(pos);
    vel_pub.publish(vel);

    ros::spinOnce();
    loop_rate.sleep();
  }

  return 0;
}